#include "PID.h"



// 左轮 PID 初始化
PIDW_Type PIDw_Left_Motor = {   
    .Kp         = 6.0f,
    .Ki         = 3.0f,
    .Kd         = 0.0f
};  // 左轮 PID

// 右轮 PID 初始化
PIDW_Type PIDw_Right_Motor = {     
    .Kp         = 6.0f,
    .Ki         = 3.0f,
    .Kd         = 0.0f
};  // 右轮 PID


//增量式PID
PIDZ_Type PIDz_Left_Motor = {
    .Kp         = 6.0f,
    .Ki         = 3.0f,
    .Kd         = 0.0f
};

PIDZ_Type PIDz_Right_Motor = {
    .Kp         = 6.0f,
    .Ki         = 3.0f,
    .Kd         = 0.0f
};


//通用位置式PID计算控制
void PIDW_Calculate(PIDW_Type *PIDw,float Actual_Value)
{
    PIDw->Actual = Actual_Value;       //获取当前实际速度
    PIDw->Last_Err = PIDw->Now_Err;         //传递误差
    PIDw->Now_Err = PIDw->Target - PIDw->Actual;    //计算当前误差
    
    if(PIDw->Ki != 0){    //避免调参时的积分饱和问题
        PIDw->Inter_Err += PIDw->Now_Err;       //累计误差
    }else{
        PIDw->Inter_Err = 0;
    }

    PIDw->Out = PIDw->Kp * PIDw->Now_Err
                  + PIDw->Ki * PIDw->Inter_Err
                  + PIDw->Kd * (PIDw->Now_Err - PIDw->Last_Err);
    
    //输出限幅
    if(PIDw->Out > 1000){PIDw->Out = 1000;}
    if(PIDw->Out < -1000){PIDw->Out = -1000;}
}


//增量式PID计算控制
void PIDZ_Calculate(PIDZ_Type *PIDz,float Actual_Value)
{
    PIDz->Actual = Actual_Value;       //获取当前实际速度
    PIDz->LastTwo_Err = PIDz->Last_Err;     //传递二次误差
    PIDz->Last_Err = PIDz->Now_Err;         //传递误差
    PIDz->Now_Err = PIDz->Target - PIDz->Actual;    //计算当前误差
    
    PIDz->Out += PIDz->Kp * PIDz->Now_Err - PIDz->Last_Err
                  + PIDz->Ki * PIDz->Now_Err
                  + PIDz->Kd * (PIDz->Now_Err - 2*PIDz->Last_Err + PIDz->LastTwo_Err);
    
    //输出限幅
    if(PIDz->Out > 1000){PIDz->Out = 1000;}
    if(PIDz->Out < -1000){PIDz->Out = -1000;}
}



